home *** CD-ROM | disk | FTP | other *** search
/ Aminet 41 / Aminet 41 (2001)(Schatztruhe)[!][Feb 2001].iso / Aminet / mus / play / dmdev.lha / delfinampeg.device / src / delfinampeg.c < prev    next >
C/C++ Source or Header  |  2000-11-04  |  21KB  |  615 lines
  1. /*****************************************************************************
  2.  
  3.     delfinampeg.device - mpeg.device for Delfina DSP
  4.     Copyright (C) 2000  Michael Henke
  5.  
  6.     This program is free software; you can redistribute it and/or modify
  7.     it under the terms of the GNU General Public License as published by
  8.     the Free Software Foundation; either version 2 of the License, or
  9.     (at your option) any later version.
  10.  
  11.     This program is distributed in the hope that it will be useful,
  12.     but WITHOUT ANY WARRANTY; without even the implied warranty of
  13.     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14.     GNU General Public License for more details.
  15.  
  16.     You should have received a copy of the GNU General Public License
  17.     along with this program; if not, write to the Free Software
  18.     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  19.  
  20. *****************************************************************************/
  21.  
  22.  
  23. /*#define DEBUG*/
  24.  
  25.  
  26. #include <proto/exec.h>
  27. #include <exec/memory.h>
  28. #include <exec/devices.h>
  29. #include <exec/errors.h>
  30. #include <exec/interrupts.h>
  31. #include <devices/melodympeg.h>
  32.  
  33. #ifdef DEBUG
  34. extern void KPutStr(STRPTR);
  35. extern LONG KPutChar(LONG);
  36. #endif
  37.  
  38. extern struct DelfObj DSP56K_PCM;
  39. extern struct DelfObj DSP56K_MP2;
  40. extern struct DelfObj DSP56K_MP3;
  41. #include <libraries/delfina.h>
  42. #include "PCM.h"
  43. #include "MP2.h"
  44. #include "MP3.h"
  45.  
  46. /** static LONG pow43tab[8206]; **/
  47. #include "MP3_pow43tab.h"
  48. /** static LONG quantab[512]; **/
  49. #include "MP3_quantab.h"
  50.  
  51. extern char DevName;    /* "delfinampeg.device" */
  52. extern struct Library *DelfinaBase; /* initialized by device Init routine */
  53.  
  54. struct devunit
  55. {
  56.     struct Unit unit;
  57.     ULONG   unitnum;
  58.     struct List ioreqlist;
  59.     struct IOMPEGReq *currioreq;
  60.     UBYTE   *currpt;
  61.     ULONG   currlen;
  62.     ULONG   volumeleft, volumeright;
  63.     ULONG   layer, freqidx, mono, firstheader;
  64.     ULONG   initdelf_ok, pause;
  65.     struct DelfPrg *prg_pcm, *prg_mp2, *prg_mp3;
  66.     struct DelfModule *mod_pcm;
  67.     DELFPTR mem_il_mp2, mem_ip_mp2;
  68.     DELFPTR mem_il_mp3, mem_ip_mp3, mem_x_mp3_ro, mem_y_mp3_quantab;
  69.     struct Interrupt delfint;
  70.     ULONG   intkey;
  71.     UBYTE   framebuf[4096], bitresbuf[4096];
  72.     UWORD   bitresoffset, bitresok, framebufstate;
  73.     UWORD   framebufoffset, framebufleft, III_main_data_size;
  74.     UWORD   delfcopysize, II_translate, II_jsbound, modext;
  75.     UBYTE   *delfcopypt;
  76. };
  77.  
  78. /* possible values for 'framebufstate' */
  79. #define FBS_GETHEADER           0
  80. #define FBS_GETFRAMEDATA        1
  81. #define FBS_FILLED              2
  82.  
  83. #define MPG_MD_STEREO           0
  84. #define MPG_MD_JOINT_STEREO     1
  85. #define MPG_MD_DUAL_CHANNEL     2
  86. #define MPG_MD_MONO             3
  87. #define HDR_MPEG1               0xfff80000
  88. #define HDR_CONSTANT            0xfffe0c00  /* layer, sampling frequency */
  89.  
  90. void* C_initunit(ULONG unitnum);
  91. ULONG C_expungeunit(struct devunit *unit);
  92. void  C_setpause(struct devunit *unit, struct IOMPEGReq *iomr);
  93. void  C_setvolume(struct devunit *unit, struct IOMPEGReq *iomr);
  94. void  C_flush(struct devunit *unit);
  95. ULONG C_write(struct devunit *unit, struct IOMPEGReq *iomr);
  96. static ULONG initDelfina(struct devunit *unit);
  97. static void  cleanupDelfina(struct devunit *unit);
  98. static void __asm IntServer(register __a1 struct devunit *unit);
  99. static UWORD GetBits(UWORD num);
  100.  
  101. static ULONG volumeleft=0x400000, volumeright=0x400000;  /* default: 100% */
  102. static UWORD *gb_pt=NULL, gb_buf=0, gb_num=0;
  103.  
  104. static const ULONG mpgfreq[4]={44100,48000,32000,0};
  105. static const UWORD mpgbitrate[3][16]=
  106.         { {0,32,64,96,128,160,192,224,256,288,320,352,384,416,448,0}, /* I */
  107.           {0,32,48,56,64,80,96,112,128,160,192,224,256,320,384,0},   /* II */
  108.           {0,32,40,48,56,64,80,96,112,128,160,192,224,256,320,0} }; /* III */
  109. static const UWORD mp2translate[3][2][16] =
  110.         { { { 0,2,2,2,2,2,2,0,0,0,1,1,1,1,1,0 } ,    /* 44100 stereo */
  111.             { 0,2,2,0,0,0,1,1,1,1,1,1,1,1,1,0 } } ,  /* 44100 mono   */
  112.           { { 0,2,2,2,2,2,2,0,0,0,0,0,0,0,0,0 } ,    /* 48000 stereo */
  113.             { 0,2,2,0,0,0,0,0,0,0,0,0,0,0,0,0 } } ,  /* 48000 mono   */
  114.           { { 0,3,3,3,3,3,3,0,0,0,1,1,1,1,1,0 } ,    /* 32000 stereo */
  115.             { 0,3,3,0,0,0,1,1,1,1,1,1,1,1,1,0 } } }; /* 32000 mono   */
  116. static const UWORD mp2sblimit[4]={27,30,8,12};
  117.  
  118.  
  119.  
  120. /** called by OpenDevice() **/
  121. void* C_initunit(ULONG unitnum)
  122. {
  123.     struct devunit *unit;
  124. #ifdef DEBUG
  125. KPutStr("C_initunit\n");
  126. #endif
  127.     unit=(struct devunit*)AllocVec(sizeof(struct devunit),MEMF_PUBLIC|MEMF_CLEAR);
  128.     if(!unit) return(unit);
  129.     unit->unit.unit_MsgPort.mp_Node.ln_Type=NT_MSGPORT;
  130.     unit->unit.unit_MsgPort.mp_Node.ln_Name= &DevName;
  131.     unit->unit.unit_MsgPort.mp_Flags=PA_IGNORE;
  132.     NewList(&unit->unit.unit_MsgPort.mp_MsgList);
  133.     unit->unitnum=unitnum;
  134.     unit->volumeleft=volumeleft;
  135.     unit->volumeright=volumeright;
  136.     NewList(&unit->ioreqlist);
  137.     return(unit);
  138. }
  139.  
  140.  
  141.  
  142. /** called by CloseDevice() **/
  143. ULONG C_expungeunit(struct devunit *unit)
  144. {
  145.     ULONG unitnum=unit->unitnum;
  146. #ifdef DEBUG
  147. KPutStr("C_expungeunit\n");
  148. #endif
  149.     C_flush(unit);
  150.     FreeVec(unit);
  151.     return(unitnum);
  152. }
  153.  
  154.  
  155.  
  156. /** called by MPEGCMD_PAUSE **/
  157. void C_setpause(struct devunit *unit, struct IOMPEGReq *iomr)
  158. {
  159. #ifdef DEBUG
  160. KPutStr("C_setpause\n");
  161. #endif
  162.     unit->pause=iomr->iomr_Arg1;
  163. }
  164.  
  165.  
  166.  
  167. /** called by MPEGCMD_SETAUDIOPARAMS **/
  168. void C_setvolume(struct devunit *unit, struct IOMPEGReq *iomr)
  169. {
  170.     struct MPEGAudioParams *ap;
  171. #ifdef DEBUG
  172. KPutStr("C_setvolume\n");
  173. #endif
  174.     ap=(struct MPEGAudioParams*)iomr->iomr_Req.io_Data;
  175.     if(ap)
  176.     {
  177.         unit->volumeleft =volumeleft =(0x400000*((ap->map_VolumeLeft >>8)&0xff))/0xff;
  178.         unit->volumeright=volumeright=(0x400000*((ap->map_VolumeRight>>8)&0xff))/0xff;
  179.         if(unit->prg_pcm)
  180.         {
  181.             Delf_Run(unit->prg_pcm->prog+PROG_PCM_INIT, 0, 0,
  182.                  unit->mono,        /*_forcemono_ is not implemented yet*/
  183.                  unit->volumeleft,
  184.                  mpgfreq[unit->freqidx],
  185.                  unit->volumeright );
  186.         }
  187.     }
  188. }
  189.  
  190.  
  191.  
  192. /** called by CMD_FLUSH **/
  193. void C_flush(struct devunit *unit)
  194. {
  195.     struct IOMPEGReq *iomr;
  196. #ifdef DEBUG
  197. KPutStr("C_flush\n");
  198. #endif
  199.     cleanupDelfina(unit);
  200.     unit->currlen=0; unit->currpt=NULL; 
  201.     unit->layer=unit->freqidx=unit->mono=unit->firstheader=0;
  202.     unit->bitresoffset=unit->bitresok=0;
  203.     unit->framebufstate=FBS_GETHEADER;
  204.     if(unit->currioreq)
  205.     {
  206.         unit->currioreq->iomr_Req.io_Error=IOERR_ABORTED;
  207.         ReplyMsg(&unit->currioreq->iomr_Req.io_Message);
  208.         unit->currioreq=NULL;
  209.     }
  210.     for(;;)
  211.     {
  212.         if(!(iomr=(struct IOMPEGReq*)RemHead(&unit->ioreqlist))) break;
  213.         iomr->iomr_Req.io_Error=IOERR_ABORTED;
  214.         ReplyMsg(&iomr->iomr_Req.io_Message);
  215.     }
  216. }
  217.  
  218.  
  219.  
  220. /** called by CMD_WRITE **/
  221. ULONG C_write(struct devunit *unit, struct IOMPEGReq *iomr)
  222. {
  223. #ifdef DEBUG
  224. /*KPutStr("C_write\n");*/
  225. #endif
  226.     if( (iomr->iomr_Req.io_Length==0) ||
  227.         (iomr->iomr_Req.io_Data==NULL) ||
  228.         (iomr->iomr_StreamType!=MPEGSTREAM_AUDIO) ) return(1); /*ERROR*/
  229.     Disable();
  230.     AddTail(&unit->ioreqlist, (struct Node*)iomr);
  231.     Enable();
  232. #ifdef DEBUG
  233. KPutStr("C_write...AddTail\n");
  234. #endif
  235.     /*even if initDelfina() returns an error we must not return it  */
  236.     /*here because the request is already queued in our ioreqlist.  */
  237.     /*(the requests in ioreqlist will be replied by C_flush() later)*/
  238.     if(!unit->initdelf_ok) initDelfina(unit);
  239.     return(0);
  240. }
  241.  
  242.  
  243.  
  244. /** called by C_write() **/
  245. static ULONG initDelfina(struct devunit *unit)
  246. {
  247.     while(!unit->firstheader)
  248.     {
  249.         ULONG header, i;
  250.         UBYTE *pt, *ptmax;
  251. #ifdef DEBUG
  252. KPutStr("initDelfina...firstheader\n");
  253. #endif
  254.         /** find frame header and extract some info: layer, freqidx, mono **/
  255.         if(!unit->currioreq)
  256.         {
  257.             if(!(unit->currioreq=(struct IOMPEGReq*)RemHead(&unit->ioreqlist))) return(1); /*ERROR*/
  258.             unit->currpt=unit->currioreq->iomr_Req.io_Data;
  259.             unit->currlen=unit->currioreq->iomr_Req.io_Length;
  260.         }
  261.         pt=unit->currpt; ptmax=pt+unit->currlen; header=0;
  262. retry_firstheader:
  263.         i=0;
  264.         while(pt<ptmax)
  265.         {
  266.             header=(header<<8)|(ULONG)(*pt++);
  267.             if( ((header&HDR_MPEG1)==HDR_MPEG1) &&  /*sync*/
  268.                 (((header>>17)&3)!=0) &&            /*layer!=4*/
  269.                 (((header>>17)&3)!=3) &&            /*layer!=1*/
  270.                 (((header>>12)&15)!=0) &&           /*bitrate!=0*/
  271.                 (((header>>12)&15)!=15) &&          /*bitrate!=15*/
  272.                 (((header>>10)&3)!=3)               /*freqidx!=3*/
  273.               ) {i=1; break;} /*pattern match*/
  274.         }
  275.         if(i) /*found something!*/
  276.         {
  277.             /**check next header (for safer recognition!)**/
  278.             i=((ULONG)mpgbitrate[3-((header>>17)&3)][(header>>12)&15]*144000)/mpgfreq[(header>>10)&3]+((header>>9)&1)-4;
  279.             if((pt+i)>=(ptmax-4)) goto retry_firstheader; /*beyond this buffer!*/
  280.             i=((ULONG)(*(pt+i))<<24)|((ULONG)(*(pt+i+1))<<16)|((ULONG)(*(pt+i+2))<<8)|(ULONG)(*(pt+i+3));
  281.             if( ((i&HDR_CONSTANT)!=(header&HDR_CONSTANT)) ||
  282.                 (((i>>12)&15)==0) ||
  283.                 (((i>>12)&15)==15) ) goto retry_firstheader; /*header mismatch!*/
  284.             /**now we are quite sure that it really is an MPEG frame header**/
  285.             unit->currpt=pt-4;
  286.             unit->currlen=ptmax-pt+4;
  287.             unit->firstheader=header;
  288.             unit->layer=4-((header>>17)&3);
  289.             unit->freqidx=(header>>10)&3;
  290.             unit->mono=( (((header>>6)&3)==MPG_MD_MONO) ? 1 : 0 );
  291.         }
  292.         else /*not found*/
  293.         {
  294.             ReplyMsg(&unit->currioreq->iomr_Req.io_Message);
  295.             unit->currioreq=NULL; unit->currpt=NULL; unit->currlen=0;
  296.         }
  297.     }
  298.  
  299.     if(!unit->prg_pcm)
  300.     {
  301. #ifdef DEBUG
  302. KPutStr("initDelfina...prg_pcm\n");
  303. #endif
  304.         if(!(unit->prg_pcm=Delf_AddPrg(&DSP56K_PCM))) return(1); /*ERROR*/
  305.         Delf_Run(unit->prg_pcm->prog+PROG_PCM_INIT, 0, 0,
  306.                  unit->mono,        /*_forcemono_ is not implemented yet*/
  307.                  unit->volumeleft,
  308.                  mpgfreq[unit->freqidx],
  309.                  unit->volumeright );
  310.     }
  311.  
  312.     if(unit->layer==2)
  313.     {
  314.         if(!unit->mem_il_mp2) unit->mem_il_mp2=Delf_AllocMem(INTL_MP2_DATL, DMEMF_LDATA|DMEMF_INTERNAL|DMEMF_ALIGN_64);
  315.         if(!unit->mem_ip_mp2) unit->mem_ip_mp2=Delf_AllocMem(INTP_MP2_PROG, DMEMF_PROG|DMEMF_INTERNAL);
  316.         if(!unit->prg_mp2)
  317.         {
  318. #ifdef DEBUG
  319. KPutStr("initDelfina...prg_mp2\n");
  320. #endif
  321.             if(!(unit->prg_mp2=Delf_AddPrg(&DSP56K_MP2))) return(1); /*ERROR*/
  322.             Delf_Run(unit->prg_mp2->prog+PROG_MP2_INIT, 0, 0, unit->mem_il_mp2, unit->mem_ip_mp2, 0, 0);
  323.         }
  324.     }
  325.     else if(unit->layer==3)
  326.     {
  327.         if(!unit->mem_il_mp3) unit->mem_il_mp3=Delf_AllocMem(INTL_MP3_DATL, DMEMF_LDATA|DMEMF_INTERNAL|DMEMF_ALIGN_64);
  328.         if(!unit->mem_ip_mp3) unit->mem_ip_mp3=Delf_AllocMem(INTP_MP3_PROG, DMEMF_PROG|DMEMF_INTERNAL);
  329.         if(!unit->mem_x_mp3_ro)
  330.         {
  331.             if(!(unit->mem_x_mp3_ro=Delf_AllocMem(EXTX_MP3_RO,DMEMF_XDATA))) return(1); /*ERROR*/
  332.         }
  333.         if(!unit->mem_y_mp3_quantab)
  334.         {
  335.             if(!(unit->mem_y_mp3_quantab=Delf_AllocMem(EXTY_MP3_QUANTAB,DMEMF_YDATA))) return(1); /*ERROR*/
  336.             Delf_CopyMem(quantab, (void*)unit->mem_y_mp3_quantab, 512*4, DCPF_FROM_AMY|DCPF_YDATA|DCPF_32BIT);
  337.         }
  338.         if(!unit->prg_mp3)
  339.         {
  340. #ifdef DEBUG
  341. KPutStr("initDelfina...prg_mp3\n");
  342. #endif
  343.             if(!(unit->prg_mp3=Delf_AddPrg(&DSP56K_MP3))) return(1); /*ERROR*/
  344.             Delf_Run(unit->prg_mp3->prog+PROG_MP3_INIT, 0, 0, unit->mem_il_mp3, unit->mem_ip_mp3, unit->freqidx, unit->mem_x_mp3_ro);
  345.             Delf_CopyMem(pow43tab, (void*)(unit->prg_mp3->ydata+DATY_MP3_POW43TAB), 8206*4, DCPF_FROM_AMY|DCPF_YDATA|DCPF_32BIT);
  346.             Delf_Poke(unit->prg_mp3->xdata+DATX_MP3_QUANTAB_P, DMEMF_XDATA, unit->mem_y_mp3_quantab);
  347.         }
  348.     }
  349.     else return(1); /*ERROR: unsupported layer*/
  350.  
  351.     if(!unit->intkey)
  352.     {
  353. #ifdef DEBUG
  354. KPutStr("initDelfina...AddIntServer\n");
  355. #endif
  356.         unit->delfint.is_Code=(void(*)(void))IntServer;
  357.         unit->delfint.is_Data=unit;
  358.         if(!(unit->intkey=Delf_AddIntServer(unit->prg_pcm->prog+PROG_PCM_INTKEY,&unit->delfint))) return(1); /*ERROR*/
  359.     }
  360.     if(!unit->mod_pcm)
  361.     {
  362. #ifdef DEBUG
  363. KPutStr("initDelfina...AddModule\n");
  364. #endif
  365.         if(!(unit->mod_pcm=Delf_AddModule(DM_Inputs, 0, DM_Outputs, 1,
  366.                             DM_Code, unit->prg_pcm->prog+PROG_PCM_MODULE,
  367.                             DM_Freq, mpgfreq[unit->freqidx], /*_dacrate_ is not implemented yet*/
  368.                             DM_Name, &DevName,
  369.                             0 ))) return(1); /*ERROR*/
  370.     }
  371.  
  372.     unit->initdelf_ok=1;
  373. #ifdef DEBUG
  374. KPutStr("initDelfina...ok\n");
  375. #endif
  376.     return(0);
  377. }
  378.  
  379.  
  380.  
  381. /** called by C_flush() **/
  382. static void cleanupDelfina(struct devunit *unit)
  383. {
  384. #ifdef DEBUG
  385. KPutStr("cleanupDelfina\n");
  386. #endif
  387.     if(unit->mod_pcm) {Delf_RemModule(unit->mod_pcm); unit->mod_pcm=NULL;}
  388.     if(unit->intkey)  {Delf_RemIntServer(unit->intkey); unit->intkey=0;}
  389.     if(unit->prg_mp2) {Delf_RemPrg(unit->prg_mp2); unit->prg_mp2=NULL;}
  390.     if(unit->prg_mp3) {Delf_RemPrg(unit->prg_mp3); unit->prg_mp3=NULL;}
  391.     if(unit->prg_pcm) {Delf_RemPrg(unit->prg_pcm); unit->prg_pcm=NULL;}
  392.     if(unit->mem_il_mp2) {Delf_FreeMem(unit->mem_il_mp2,DMEMF_LDATA|DMEMF_INTERNAL); unit->mem_il_mp2=NULL;}
  393.     if(unit->mem_ip_mp2) {Delf_FreeMem(unit->mem_ip_mp2,DMEMF_PROG |DMEMF_INTERNAL); unit->mem_ip_mp2=NULL;}
  394.     if(unit->mem_il_mp3) {Delf_FreeMem(unit->mem_il_mp3,DMEMF_LDATA|DMEMF_INTERNAL); unit->mem_il_mp3=NULL;}
  395.     if(unit->mem_ip_mp3) {Delf_FreeMem(unit->mem_ip_mp3,DMEMF_PROG |DMEMF_INTERNAL); unit->mem_ip_mp3=NULL;}
  396.     if(unit->mem_x_mp3_ro) {Delf_FreeMem(unit->mem_x_mp3_ro,DMEMF_XDATA); unit->mem_x_mp3_ro=NULL;}
  397.     if(unit->mem_y_mp3_quantab) {Delf_FreeMem(unit->mem_y_mp3_quantab,DMEMF_YDATA); unit->mem_y_mp3_quantab=NULL;}
  398.     unit->initdelf_ok=0;
  399. }
  400.  
  401.  
  402.  
  403. static void __asm IntServer(register __a1 struct devunit *unit)
  404. {
  405.     if(unit->pause || (unit->framebufstate!=FBS_FILLED))
  406.     {
  407.         Delf_Run(unit->prg_pcm->prog+PROG_PCM_MUTE,0,DRUNF_ASYNCH,0,0,0,0);
  408.     }
  409.     else
  410.     {
  411.         if(unit->layer==2)
  412.         {
  413.             if(!Delf_Peek(unit->prg_mp2->ydata+DATY_MP2_BUSY,DMEMF_YDATA))
  414.             {
  415.                 Delf_CopyMem( unit->delfcopypt,
  416.                               (void*)(unit->prg_mp2->xdata+DATX_MP2_INBUF),
  417.                               (ULONG)unit->delfcopysize,
  418.                               DCPF_FROM_AMY|DCPF_XDATA|DCPF_24BIT );
  419.                 Delf_Run( unit->prg_mp2->prog+PROG_MP2_DECODE, 0, DRUNF_ASYNCH,
  420.                           unit->mono,
  421.                           Delf_Peek(unit->prg_pcm->ydata+DATY_PCM_BUFPTR,DMEMF_YDATA),
  422.                           (ULONG)unit->II_translate,
  423.                           (ULONG)unit->II_jsbound );
  424.                 unit->framebufstate=FBS_GETHEADER;
  425.             }
  426.         }
  427.         else if(unit->layer==3)
  428.         {
  429.             if(!Delf_Peek(unit->prg_mp3->ydata+DATY_MP3_BUSY,DMEMF_YDATA))
  430.             {
  431.                 if(unit->bitresok)
  432.                 {
  433.                     Delf_CopyMem( &unit->bitresbuf[0],
  434.                                   (void*)(unit->prg_mp3->xdata+DATX_MP3_INBUF),
  435.                                   (ULONG)(unit->III_main_data_size+33),
  436.                                   DCPF_FROM_AMY|DCPF_XDATA|DCPF_24BIT );
  437.                     Delf_Run( unit->prg_mp3->prog+PROG_MP3_DECODE, 0, DRUNF_ASYNCH,
  438.                               unit->mono,
  439.                               Delf_Peek(unit->prg_pcm->ydata+DATY_PCM_BUFPTR,DMEMF_YDATA),
  440.                               (ULONG)unit->modext,
  441.                               0 );
  442.                 }
  443.                 unit->framebufstate=FBS_GETHEADER;
  444.             }
  445.         }
  446.     }
  447.  
  448.     if(unit->framebufstate==FBS_GETHEADER)
  449.     {
  450.         /*find next valid frame header*/
  451.         ULONG fh=unit->firstheader&HDR_CONSTANT, ch=0;
  452.         while( ((ch&HDR_CONSTANT)!=fh) ||   /*sync, layer, freqidx*/
  453.                (((ch>>12)&15)==0) ||        /*bitrate!=0*/
  454.                (((ch>>12)&15)==15) )        /*bitrate!=15*/
  455.         {
  456.             if(unit->currioreq)
  457.             {
  458.                 if(unit->currlen)
  459.                 {
  460.                     ch=(ch<<8)|(ULONG)(*unit->currpt);
  461.                     unit->currpt++; unit->currlen--;
  462.                 }
  463.                 else
  464.                 {
  465.                     ReplyMsg(&unit->currioreq->iomr_Req.io_Message);
  466.                     unit->currioreq=NULL; unit->currpt=NULL; /*unit->currlen=0;*/
  467.                 }
  468.             }
  469.             else
  470.             {
  471.                 if(!(unit->currioreq=(struct IOMPEGReq*)RemHead(&unit->ioreqlist)))
  472.                 {
  473.                     ch=0; break; /*ERROR*/
  474.                 }
  475.                 unit->currpt=unit->currioreq->iomr_Req.io_Data;
  476.                 unit->currlen=unit->currioreq->iomr_Req.io_Length;
  477.             }
  478.         }
  479.         if(ch) /*found it!*/
  480.         {
  481.             unit->framebufoffset=0;
  482.             unit->framebufleft=((ULONG)mpgbitrate[unit->layer-1][(ch>>12)&15]*144000)/mpgfreq[(ch>>10)&3]+((ch>>9)&1)-4;
  483.             unit->II_translate=mp2translate[unit->freqidx][unit->mono][(ch>>12)&15];
  484.             if(((ch>>6)&3)==MPG_MD_JOINT_STEREO)
  485.             {
  486.                 unit->modext=(ch>>4)&3;
  487.                 unit->II_jsbound=(unit->modext<<2)+4;
  488.             }
  489.             else
  490.             {
  491.                 unit->modext=0;
  492.                 unit->II_jsbound=mp2sblimit[unit->II_translate];
  493.             }
  494.             unit->delfcopysize=unit->framebufleft;
  495.             unit->delfcopypt= &unit->framebuf[0];
  496.             if(!((ch>>16)&1))
  497.             {
  498.                 unit->delfcopysize-=2;
  499.                 unit->delfcopypt+=2;
  500.             }
  501.             unit->framebufstate=FBS_GETFRAMEDATA;
  502.         }
  503.     }
  504.  
  505.     if(unit->framebufstate==FBS_GETFRAMEDATA)
  506.     {
  507.         /*fetch frame data*/
  508. #ifdef DEBUG
  509. KPutChar('i');
  510. #endif
  511.         while(unit->framebufleft)
  512.         {
  513.             if(unit->currioreq)
  514.             {
  515.                 if(unit->currlen)
  516.                 {
  517.                     ULONG i;
  518.                     if(unit->framebufleft<unit->currlen) i=unit->framebufleft;
  519.                     else i=unit->currlen;
  520.                     CopyMem(unit->currpt, &unit->framebuf[unit->framebufoffset], i);
  521.                     unit->currpt+=i; unit->currlen-=i;
  522.                     unit->framebufoffset+=i; unit->framebufleft-=i;
  523.                     if(unit->framebufleft==0) unit->framebufstate=FBS_FILLED;
  524.                 }
  525.                 else
  526.                 {
  527.                     ReplyMsg(&unit->currioreq->iomr_Req.io_Message);
  528.                     unit->currioreq=NULL; unit->currpt=NULL; /*unit->currlen=0;*/
  529.                 }
  530.             }
  531.             else
  532.             {
  533.                 if(!(unit->currioreq=(struct IOMPEGReq*)RemHead(&unit->ioreqlist)))
  534.                 {
  535.                     break; /*ERROR*/
  536.                 }
  537.                 unit->currpt=unit->currioreq->iomr_Req.io_Data;
  538.                 unit->currlen=unit->currioreq->iomr_Req.io_Length;
  539.             }
  540.         }
  541.         if((unit->framebufstate==FBS_FILLED) && (unit->layer==3))
  542.         {
  543.             UWORD md;
  544.             /**extract main data size**/
  545.             gb_pt=(UWORD*)(unit->delfcopypt+2); gb_num=0;
  546.             if(unit->mono)
  547.             {
  548.                 GetBits(9+5+4-16);
  549.                 md=GetBits(12);
  550.             }
  551.             else
  552.             {
  553.                 GetBits(9+3+8-16);
  554.                 md=GetBits(12);
  555.                 GetBits(59-12);
  556.                 md+=GetBits(12);
  557.                 GetBits(59-12);
  558.                 md+=GetBits(12);
  559.             }
  560.             GetBits(59-12);
  561.             md+=GetBits(12);
  562.             unit->III_main_data_size=(md+7)>>3;
  563.             /**bit reservoir handling**/
  564.             {
  565.                 UWORD main_data_begin, i;
  566.                 UBYTE *p0, *p1;
  567.                 p1=unit->delfcopypt;
  568.                 main_data_begin= (UWORD)(*p1)<<1 | (UWORD)(*(p1+1))>>7;
  569.                 /* copy side information */
  570.                 p0= &unit->bitresbuf[0];
  571.                 for(i=32; i>0; i--) *p0++= *p1++;
  572.                 /* copy previous main_data */
  573.                 p0++; /* &unit->bitresbuf[33] */
  574.                 if(unit->bitresoffset>=main_data_begin)
  575.                 {
  576.                     unit->bitresok=1;
  577.                     p1=p0+unit->bitresoffset-main_data_begin;
  578.                     for(i=main_data_begin; i>0; i--) *p0++= *p1++;
  579.                     unit->bitresoffset=main_data_begin;
  580.                 }
  581.                 else
  582.                 {
  583.                     unit->bitresok=0; /* not enough data in reservoir */
  584.                     p0+=unit->bitresoffset;
  585.                 }
  586.                 /* copy current main_data */
  587.                 i=(unit->mono) ? 17 : 32; /* side info length */
  588.                 p1=unit->delfcopypt+i;    /* begin of main_data area */
  589.                 i=unit->delfcopysize-i;   /* mainslots */
  590.                 unit->bitresoffset+=i;
  591.                 for(; i>0; i--) *p0++= *p1++;
  592.             }
  593.         }
  594.     }
  595. }
  596.  
  597.  
  598.  
  599. static UWORD GetBits(UWORD num)
  600. {
  601.     UWORD val=0;
  602.     for(; num>0; num--)
  603.     {
  604.         if(gb_num==0)
  605.         {
  606.             gb_buf= *gb_pt++;
  607.             gb_num= 16;
  608.         }
  609.         gb_num--;
  610.         val+= val;
  611.         val|= (gb_buf>>gb_num)&1;
  612.     }
  613.     return(val);
  614. }
  615.